/*
* Make sure Wire.begin() has been called in the setup() method of your sketch
*/

#include "WProgram.h"
#include "HMC6352.h"
#include "Wire.h"

HMC6352::HMC6352()
{

}

HMC6352::HMC6352(int address, int smoothSample, int VCCPin1, int VCCPin2)
{
 _smoothSample=smoothSample;
 _slaveAddress=address>>1; // shifts the given add 1B to the right to work with TWI lib
 // alim :
 pinMode(VCCPin1, OUTPUT);
 pinMode(VCCPin2, OUTPUT);
 digitalWrite(VCCPin1, LOW);
 digitalWrite(VCCPin2, HIGH);
}

float HMC6352::headingDeg(){
 byte headingData[2];
 Wire.beginTransmission(_slaveAddress);
  Wire.send("A");
  Wire.endTransmission();
  delay(7); // TODO: test with lower values
  Wire.requestFrom(_slaveAddress,2);
  int i=0;
  while(Wire.available()&&i<2)
  {
   headingData[i]=Wire.receive();
   i++;
  }
 _headingDeg=(headingData[0]*256 + headingData[1])/10.;
 return _headingDeg;
}

float HMC6352::headingSmoothDeg()
{
 _headingSmoothDeg=0;
 for(int i=0;i<_smoothSample;i++)
 {
  _headingSmoothDeg+=headingDeg();
 }
 _headingSmoothDeg/=_smoothSample;
 return _headingSmoothDeg;
}

Vector2D HMC6352::headingSmoothVect()
{
 Serial.print("heading in deg: ");
Serial.println(headingSmoothDeg());
 _headingSmoothVect=Vector2D(sin(radians(_headingSmoothDeg)),cos(radians(_headingSmoothDeg)));
// _headingSmoothVect._y=cos(_headingSmoothDeg);
// _headingSmoothVect._x=sin(_headingSmoothDeg);
 return _headingSmoothVect;
}

Vector2D HMC6352::headingVect()
{
 headingDeg();
 //_headingVect._y=cos(radians(_headingDeg));
 //_headingVect._x=sin(radians(_headingDeg));
 _headingVect=Vector2D(sin(radians(_headingDeg)),cos(radians(_headingDeg)));
 return _headingVect;
}

void HMC6352::calibrate(int timeMs)
{
 Wire.beginTransmission(_slaveAddress);
 Wire.send("C"); // enter user calibration mode
 delay(timeMs);
 Wire.send("E"); // exit user calibration mode
 Wire.endTransmission();
}

